/***************************************************************************************************************************
* sylu_terminal_base.coo
*
* Author: Wxt（移植于prometheus）
*
* Update Time: 2021.06.06
*
* Introduction:  发送控制指令（终端交互）
*                               修改交互逻辑：1.取消999（rotors没有offboard，后续的Tello也不会有）
*
***************************************************************************************************************************/
#include <ros/ros.h>
#include <iostream>
#include <uav_drone_msgs/ControlCommand.h>

#include "KeyboardEvent.h"

#define VEL_XY_STEP_SIZE 0.1
#define VEL_Z_STEP_SIZE 0.1
#define YAW_STEP_SIZE 0.08
#define TRA_WINDOW 2000
#define NODE_NAME "terminal_control"

using namespace std;

//即将发布的command
uav_drone_msgs::ControlCommand Command_to_pub;

//发布
ros::Publisher move_pub;
ros::Publisher ref_trajectory_pub;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　函数声明　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void mainloop1();
void generate_com(int Move_mode, float state_desired[4]);

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>　主函数　<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "terminal_control");
    ros::NodeHandle nh;

    //　【发布】　控制指令
    move_pub = nh.advertise<uav_drone_msgs::ControlCommand>("/sylu_drone/control_command", 10);

    // 初始化命令 - Idle模式 电机怠速旋转 等待来自上层的控制指令
    Command_to_pub.Mode                                = uav_drone_msgs::ControlCommand::Idle;
    Command_to_pub.Command_ID                          = 0;
    Command_to_pub.source = NODE_NAME;
    Command_to_pub.Reference_State.Move_mode           = uav_drone_msgs::PositionReference::XYZ_POS;
    Command_to_pub.Reference_State.Move_frame          = uav_drone_msgs::PositionReference::ENU_FRAME;
    Command_to_pub.Reference_State.position_ref[0]     = 0;
    Command_to_pub.Reference_State.position_ref[1]     = 0;
    Command_to_pub.Reference_State.position_ref[2]     = 0;
    Command_to_pub.Reference_State.velocity_ref[0]     = 0;
    Command_to_pub.Reference_State.velocity_ref[1]     = 0;
    Command_to_pub.Reference_State.velocity_ref[2]     = 0;
    Command_to_pub.Reference_State.acceleration_ref[0] = 0;
    Command_to_pub.Reference_State.acceleration_ref[1] = 0;
    Command_to_pub.Reference_State.acceleration_ref[2] = 0;
    Command_to_pub.Reference_State.yaw_ref             = 0;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(2);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    //cout.setf(ios::showpos);

    // 选择通过终端输入控制或键盘控制
    int Remote_Mode;
    cout << ">>>>>>>>>>>>>>>> Welcome to use sylu Terminal base <<<<<<<<<<<<<<<<"<< endl;
    cout << "Please input 0: start Terminal contorl"<<endl;
    cin >> Remote_Mode;

    if (Remote_Mode == 0)
    {
        cout << "Contorl is start，You can command input control mode"<<endl;
        mainloop1();
    }

    return 0;
}

void mainloop1()
{
    int Control_Mode = 0;
    int Move_mode = 0;
    int Move_frame = 0;
    int Trjectory_mode = 0;
    float state_desired[4];

    while(ros::ok())
    {
        // Waiting for input
        cout << ">>>>>>>>>>>>>>>> Welcome to use sylu Terminalbase <<<<<<<<<<<<<<<<"<< endl;
        cout << "Please choose the Command.Mode: 0 for Idle, 1 for Takeoff, 2 for Hold, 3 for Land, 4 for Move, 5 for Disarm, 6 for User_Mode1, 7 for User_Mode2"<<endl;
        cin  >> Control_Mode;

        if(Control_Mode == uav_drone_msgs::ControlCommand::Move)
        {
            
            cout << "current：The tello driver uses CMD_VEL implement only control pitch,roll,yaw,throttle"<<endl;
            cout << "Please choose the Command.Reference_State.Move_mode: 0 for XYZ_POS, 1 for XY_POS_Z_VEL, 2 for XY_VEL_Z_POS, 3 for XYZ_VEL"<<endl;
            cin >> Move_mode;

            cout << "Please choose the Command.Reference_State.Move_frame: 0 for ENU_FRAME, 1 for BODY_FRAME"<<endl;
            cin >> Move_frame; 
            cout << "Please input the reference state [x y z yaw]: "<< endl;
            cout << "setpoint_t[0] --- x [m][current roll -1 ~ 1] : "<< endl;
            cin >> state_desired[0];
            cout << "setpoint_t[1] --- y [m][current pitch -1 ~ 1]: "<< endl;
            cin >> state_desired[1];
            cout << "setpoint_t[2] --- z [m][current throttle -1 ~ 1]: "<< endl;
            cin >> state_desired[2];
            cout << "setpoint_t[3] --- yaw [du][current yaw -1 ~ 1] : "<< endl;
            cin >> state_desired[3];
            //需要注意，当前给的是一个比值，不是实际的角度，并且不知道1对应的具体角度大小
        }
        switch (Control_Mode)
        {
            case uav_drone_msgs::ControlCommand::Idle:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Idle;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;

            case uav_drone_msgs::ControlCommand::Takeoff:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Takeoff;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;

            case uav_drone_msgs::ControlCommand::Hold:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Hold;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;
    
            case uav_drone_msgs::ControlCommand::Land:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Land;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;

            case uav_drone_msgs::ControlCommand::Move:
                    Command_to_pub.header.stamp = ros::Time::now();
                    Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Move;
                    Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                    Command_to_pub.source = NODE_NAME;
                    Command_to_pub.Reference_State.Move_mode  = Move_mode;
                    Command_to_pub.Reference_State.Move_frame = Move_frame;
                    Command_to_pub.Reference_State.time_from_start = -1;

                    //generate_com可以用于将位置转换为速度进行发送
                    //generate_com(Move_mode, state_desired);

                    //根据程序赋予，【0 1 2 3】分别对应的是【X Y Z YAW】
                    Command_to_pub.Reference_State.position_ref[0]=state_desired[0];
                    Command_to_pub.Reference_State.position_ref[1]=state_desired[1];
                    Command_to_pub.Reference_State.position_ref[2]=state_desired[2];
                    //Command_to_pub.Reference_State.yaw_ref = state_desired[3];
                    Command_to_pub.Reference_State.yaw_ref = state_desired[3]/180.0*M_PI;           //偏航角在rotors中以四元数控制
                                                                                                                                                                                            //但TELLO的cmd_vel是直接给角度

        
                    move_pub.publish(Command_to_pub);
                break;
            
            case uav_drone_msgs::ControlCommand::Disarm:                                                                //使用Disarm后，不应在响应其他指令
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::Disarm;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;

            case uav_drone_msgs::ControlCommand::User_Mode1:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::User_Mode1;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;
            
            case uav_drone_msgs::ControlCommand::User_Mode2:
                Command_to_pub.header.stamp = ros::Time::now();
                Command_to_pub.Mode = uav_drone_msgs::ControlCommand::User_Mode2;
                Command_to_pub.Command_ID = Command_to_pub.Command_ID + 1;
                Command_to_pub.source = NODE_NAME;
                move_pub.publish(Command_to_pub);
                break;
        }
        
        cout << "....................................................." <<endl;
        
        sleep(1.0);
    }
}

void generate_com(int Move_mode, float state_desired[4])
{
    Command_to_pub.Reference_State.position_ref[0]=1;
    Command_to_pub.Reference_State.position_ref[1]=1;
    Command_to_pub.Reference_State.position_ref[2]=1;
    Command_to_pub.Reference_State.yaw_ref = state_desired[3]/180.0*M_PI;
}